#include <iostream>
#include <fstream>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <vector>
#include <string.h>
#include <pthread.h>
#include <sys/types.h>
#include <dirent.h>
#include <errno.h>

using namespace std;

struct groundtruth {
	double timestamp;
	double tx;
	double ty;
	double tz;
	double qx;
	double qy;
	double qz;
	double qw;
};

struct Timestamp_path {
	double timestamp;
	string path;
};

struct Frame_data {
	Timestamp_path * rgb;
	Timestamp_path * depth;
};


int getdir (string dir, vector<string> &files)
{
    DIR *dp;
    struct dirent *dirp;
    if((dp  = opendir(dir.c_str())) == NULL) {
        cout << "Error(" << errno << ") opening " << dir << endl;
        return errno;
    }

    while ((dirp = readdir(dp)) != NULL) {
        files.push_back(string(dirp->d_name));
    }
    closedir(dp);
    return 0;
}


void test()
{
	string path = "/home/johane/test_data/rgbd_dataset_freiburg1_room";

																																																																																																																																																																																																																																																																																																																																																																																	
	string path_gt = path+"/groundtruth.txt";
	string path_rgb = path+"/rgb.txt";
	string path_depth = path+"/depth.txt";

	printf("starting setup\n");
	
	string line;
	ifstream gt_file (path_gt.c_str());
	vector< groundtruth * > gt_vec;
	if (gt_file.is_open()){
		while ( gt_file.good() )
		{
			getline (gt_file,line);
			if(line[0] != '#'){
				int space1 = line.find(" ");
				if(space1 != -1){
					
					int space2 = line.find(" ",space1+1,1);
					int space3 = line.find(" ",space2+1,1);
					int space4 = line.find(" ",space3+1,1);
					int space5 = line.find(" ",space4+1,1);
					int space6 = line.find(" ",space5+1,1);
					int space7 = line.find(" ",space6+1,1);
					
					groundtruth * gt = new groundtruth();
					gt->timestamp 	= atof(line.substr(0,space1).c_str());
					gt->tx 			= atof(line.substr(space1+1,space2-space1-1).c_str());
					gt->ty 			= atof(line.substr(space2+1,space3-space2-1).c_str());
					gt->tz 			= atof(line.substr(space3+1,space4-space3-1).c_str());
					gt->qx 			= atof(line.substr(space4+1,space5-space4-1).c_str());
					gt->qy 			= atof(line.substr(space5+1,space6-space5-1).c_str());
					gt->qz 			= atof(line.substr(space6+1,space7-space6-1).c_str());
					gt->qw 			= atof(line.substr(space7+1,line.length()-space7-1).c_str());
					gt_vec.push_back(gt);
				}
			}
		}
		gt_file.close();
	}else{cout << "Unable to open " << path_gt;}
	
	
	ifstream rgb_file (path_rgb.c_str());
	vector< Timestamp_path * > rgb_vec;
	if (rgb_file.is_open()){
		while ( rgb_file.good() )
		{
			getline (rgb_file,line);
			if(line[0] != '#'){
				int space1 = line.find(" ");
				if(space1 != -1){
					Timestamp_path * tp = new Timestamp_path();
					tp->timestamp 		= atof(line.substr(0,space1).c_str());
					tp->path			= path+"/"+line.substr(space1+1,line.length()-space1-1);
					rgb_vec.push_back(tp);
					//print_tp(tp);
					//printf("Line: %s\n",line.c_str());
				}
			}
		}
		rgb_file.close();
	}else{cout << "Unable to open " << path_rgb;}
	
	ifstream depth_file (path_depth.c_str());
	vector< Timestamp_path * > depth_vec;
	if (depth_file.is_open()){
		while ( depth_file.good() )
		{
			getline (depth_file,line);
			if(line[0] != '#'){
				int space1 = line.find(" ");
				if(space1 != -1){
					Timestamp_path * tp = new Timestamp_path();
					tp->timestamp 		= atof(line.substr(0,space1).c_str());
					tp->path			= path+"/"+line.substr(space1+1,line.length()-space1-1);
					depth_vec.push_back(tp);
					//print_tp(tp);
					//printf("Line: %s\n",line.c_str());
				}
			}
		}
		depth_file.close();
	}else{cout << "Unable to open " << path_depth;}
	printf("rgb_vec: %i depth_vec: %i gt_vec: %i\n",(int)rgb_vec.size(),(int)depth_vec.size(),(int)gt_vec.size());
	printf("done reading\n");
	
	int closest_d = 0;
	int closest_t = 0;

	//vector< frame_data * > all_frames;
	for(int i = 0; i < rgb_vec.size(); i+=1)
	{
		
		printf("%i\n",i);
		float last_diff_d = 1000000;
		for(unsigned int j = closest_d; j < depth_vec.size(); j++)
		{
			float diff = fabs(rgb_vec.at(i)->timestamp - depth_vec.at(j)->timestamp);
			if(diff > last_diff_d){
				closest_d = j-1;
				break;
			}else{
				last_diff_d = diff;
			}
		}
		printf("best_d: %i, diff_d: %f\n",closest_d,last_diff_d);
		float last_diff_t = 1000000;
		for(unsigned int j = closest_t; j < gt_vec.size(); j++)
		{
			float diff = fabs(depth_vec.at(closest_d)->timestamp - gt_vec.at(j)->timestamp);
			if(diff > last_diff_t){
				closest_t = j-1;
				break;
			}else{
				last_diff_t = diff;
				closest_t =j;
			}
		}
		printf("best_gt: %i, diff_gt: %f\n",closest_t,last_diff_t);
		/*
		frame_data * fd = new frame_data();
		fd->rgb 		= rgb_vec.at(i);
		fd->depth 		= depth_vec.at(closest_d);
		fd->gt 			= gt_vec.at(closest_t);
		
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = generatePC(fd,calibration);
		pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
		pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
		ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
		ne.setMaxDepthChangeFactor(0.02f);
		ne.setNormalSmoothingSize(5.0f);
		ne.setInputCloud(cloud);
		ne.compute(*normals);
		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr full_cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
		pcl::concatenateFields(*cloud,*normals,*full_cloud);
		cloud.reset();
		normals.reset();
		fd->frame		= new RGBDFrame(full_cloud,centers);
		all_frames.push_back(fd);
		*/
	}
	
	/*
	const bool debugg_testing = false;

	for(int i = 0; i < (int)all_frames.size(); i++)
	{
		for(int j = i-1; j >=  max(0,i-max_backing); j--){
			for(unsigned int k = 0; k < matchers.size(); k++)
			{
				char fpath [150];
				sprintf(fpath,"output/%s_%i_%i_%i.task",prefix.c_str(),i,j,k);
				ifstream task_file (fpath);
				if (!task_file.is_open()){
					long size 				= sizeof(int)*(8+16);
					char * buffer_char 		= new char[size];
					int * buffer_int 		= (int *)buffer_char;
					float * buffer_float	= (float *)buffer_char;
					buffer_int[0] = i;
					buffer_int[1] = j;
					buffer_int[2] = k;
					buffer_int[3] = 0;//done?
					buffer_float[4] = 0;//val_time;
					buffer_float[5] = 0;//pos_error;
					buffer_float[6] = 0;//trans_error;
					buffer_float[7] = 0;//weight;
					
					buffer_float[8] = 0;
					buffer_float[9] = 0;
					buffer_float[10] = 0;
					buffer_float[11] = 0;
					buffer_float[12] = 0;
					buffer_float[13] = 0;
					buffer_float[14] = 0;
					buffer_float[15] = 0;
					
					buffer_float[16] = 0;
					buffer_float[17] = 0;
					buffer_float[18] = 0;
					buffer_float[19] = 0;
					buffer_float[20] = 0;
					buffer_float[21] = 0;
					buffer_float[22] = 0;
					buffer_float[23] = 0;
					
					ofstream outfile (fpath,ofstream::binary);
					outfile.write (buffer_char,size);
					outfile.close();
					delete buffer_char;
				}else{task_file.close();}
			}
		}

	}

	tasks = new vector<test_task * >();
	
	vector<string> files = vector<string>();
	getdir("output/",files);
	int added_tasks = 0;
    for (unsigned int i = 0;i < files.size();i++) {
        if(files.at(i).find(prefix)==0 && files.at(i).find("_") == prefix.length()){
        	ifstream task_file (("output/"+files.at(i)).c_str());
			if (task_file.is_open()){
				
				task_file.seekg(0,ifstream::end);
				long size=task_file.tellg();
				char * buffer_char 		= new char [size];
				int * buffer_int 		= (int *) buffer_char;
				float * buffer_float	= (float *) buffer_char;
				task_file.seekg(0);
				task_file.read (buffer_char,size);
				task_file.close();
				
				//if(i%100 == 0){
					printf("%f---->%i %i %i %i -> %f %f %f %f\n",float(i)/float(files.size()),buffer_int[0],buffer_int[1],buffer_int[2],buffer_int[3],buffer_float[4],buffer_float[5],buffer_float[6],buffer_float[7]);
					printf("Transformation :");
					for(int k = 0; k < 16; k++){printf("%f ",buffer_float[8+k]);}
					printf("\n");
				//}
				
				if(buffer_int[3] == 0){
					//printf("creating task\n");
					frame_data * src_fd = all_frames.at(buffer_int[0]);
					frame_data * dst_fd = all_frames.at(buffer_int[1]);
					test_task * t = new test_task();
					t->src = src_fd;
					t->dst = dst_fd;
					t->matcher = matchers.at(buffer_int[2]);
					t->matcher_id = buffer_int[2];
					t->filename = string(files.at(i));
					
					pthread_mutex_lock( &tasks_mutex );
					tasks->push_back(t);
					pthread_mutex_unlock(&tasks_mutex);
					added_tasks++;
				}
				delete buffer_char;
        	}
        }
    }
    pthread_mutex_lock( &tasks_mutex );
	sort(tasks->begin(),tasks->end(),mycomparison);
	pthread_mutex_unlock(&tasks_mutex);
    for(int i = 0; i < 6; i++){
		pthread_t mythread;
		pthread_create( &mythread, NULL, start_test_thread, NULL);
	}
    
    struct timeval test_start, test_end;
	gettimeofday(&test_start, NULL);
	while(nr_done_tasks() < added_tasks){
		gettimeofday(&test_end, NULL);
		float test_time = (test_end.tv_sec*1000000+test_end.tv_usec-(test_start.tv_sec*1000000+test_start.tv_usec))/1000000.0f;
		printf("%i/%i Time spent: %f\n",nr_done_tasks(),added_tasks,test_time);
		usleep(500000);
	}
*/
}
int main(int argc, char **argv)
{
	test();
	return 0;
}
